#ifndef GRIPPER_CONTROL_NODE_HPP
#define GRIPPER_CONTROL_NODE_HPP

#include <rclcpp/rclcpp.hpp>
#include "gripper_control/srv/gripper_control.hpp"
#include "gripper_control/msg/gripper_status.hpp"
#include "gripper.h"

#define TARGET_POS_OPEN 0
#define TARGET_POS_CLOSE 9000
#define TARGET_DEFAULT_TORQUE   300
#define TARGET_DEFAULT_SPEED    500

const char* ctrl_port = "/dev/ttyUSB0";

class GripperControlNode : public rclcpp::Node
{
public:
    GripperControlNode();

private:
    bool left_gripper_is_open = true;
    bool right_gripper_is_open = true;
    rclcpp::Service<gripper_control::srv::GripperControl>::SharedPtr gripper_control_srv_;
    rclcpp::Publisher<gripper_control::msg::GripperStatus>::SharedPtr status_pub_;

    void init_gripper_pos();

    void handle_gripper_request(
        const std::shared_ptr<gripper_control::srv::GripperControl::Request> request,
        const std::shared_ptr<gripper_control::srv::GripperControl::Response> response);

    void publish_gripper_status();
    // DriverSDK::GripperController controller;
    DriverSDK::GripperTarget target;
    DriverSDK::GripperStatus status;
    DriverSDK::Gripper left_gripper;
    DriverSDK::Gripper right_gripper;
    rclcpp::TimerBase::SharedPtr timer_;
};

#endif  // GRIPPER_CONTROL_NODE_HPP